#include <opencv2\core\core.hpp>
#include <opencv2\imgcodecs\imgcodecs.hpp>
#include <opencv2\imgproc\imgproc.hpp>
#include <opencv2\highgui\highgui.hpp>

#include <vector>
#include <iostream>

using namespace std;
using namespace cv;

double getMatPoint(cv::Mat & src, int i, int j, int c = 0)
{
	double* curRow = src.ptr<double>(i);
	return *(curRow + j * src.channels() + c);
}

int main()
{
	vector<Point2d> points;

	Point2d p1, p2, p3;
	p1.x = 99.47;//points[0].x;
	p2.x = 242.861;//points[10].x;
	p3.x = 491.838;//points[21].x;

	p1.y = 10.2;// points[0].y;
	p2.y = 118.2;//points[10].y;
	p3.y = 235.2;//points[21].y;

	cout << "point_1" << endl;

	double a[3][3];
	double b[3];

	a[0][0] = p1.x * p1.x;
	a[0][1] = p1.x;
	a[0][2] = 1.0;
	a[1][0] = p2.x * p2.x;
	a[1][1] = p2.x;
	a[1][2] = 1.0;
	a[2][0] = p3.x * p3.x;
	a[2][1] = p3.x;
	a[2][2] = 1.0;

	vector<float> vec_a;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			vec_a.push_back(a[i][j]);
		}
	}
	cout << "point_2" << endl;

	Mat Ma = Mat(vec_a).reshape(1, 3);

	b[0] = p1.y;
	b[1] = p2.y;
	b[2] = p3.y;

	vector<float> vec_b;
	for (int i = 0; i < 3; i++)
	{
		vec_b.push_back(b[i]);
	}
	Mat Mb = Mat(vec_b);
	cout << "point_3" << endl;

	Mat Mx;
	solve(Ma, Mb, Mx, CV_LU); // solve (Ax=b) for x   
	cout << "Mx: " << Mx.reshape(1, 1) << endl;
	Mx.convertTo(Mx, CV_64F);

	double para[3];
	para[0] = getMatPoint(Mx, 0, 0);
	para[1] = getMatPoint(Mx, 1, 0);
	para[2] = getMatPoint(Mx, 2, 0);

	cout << "para[0]: " << para[0] << endl;
	cout << "para[1]: " << para[1] << endl;
	cout << "para[2]: " << para[2] << endl;

	waitKey(0);
	return 0;
}